/**************************************************************************************************
* Title: SumQuaternion_16f.inl
* Author: Gael Huber
* Description: Implements functions useful for 16-byte aligned quaternions
**************************************************************************************************/

/**************************************************************************************************
* Default constructor
**************************************************************************************************/
inline Quaternion_16f::Quaternion_16f(void)
	: v(_mm_setzero_ps())
{ }

/**************************************************************************************************
* Constructor from a vector
**************************************************************************************************/
inline Quaternion_16f::Quaternion_16f(const __m128& f)
	: v(f)
{ }

/**************************************************************************************************
* Constructor from array
**************************************************************************************************/
inline Quaternion_16f::Quaternion_16f(const float* f)
	: v(_mm_load_ps(f))
{ }

/**************************************************************************************************
* Constructor given preset values
**************************************************************************************************/
inline Quaternion_16f::Quaternion_16f(float x, float y, float z, float w)
	: v(_mm_set_ps(w, z, y, x))
{ }

/**************************************************************************************************
* Copy constructor
**************************************************************************************************/
inline Quaternion_16f::Quaternion_16f(const Quaternion_16f& rhs)
	: v(rhs.v)
{ }

/**************************************************************************************************
* Destructor
**************************************************************************************************/
inline Quaternion_16f::~Quaternion_16f(void)
{ }

/**************************************************************************************************
* Cast to float array
**************************************************************************************************/
inline Quaternion_16f::operator float*(void) {
	return (float*) this;
}

/**************************************************************************************************
* Cast to const float array
**************************************************************************************************/
inline Quaternion_16f::operator const float*(void) const {
	return (const float*) this;
}

/**************************************************************************************************
* Assignment operator
**************************************************************************************************/
inline Quaternion_16f& Quaternion_16f::operator=(const Quaternion_16f& rhs) {
	if(&rhs == this)
		return *this;

	v = rhs.v;
	return *this;
}

/**************************************************************************************************
* Add a quaternion and return the result
**************************************************************************************************/
inline Quaternion_16f& Quaternion_16f::operator+=(const Quaternion_16f& rhs) {
	v = _mm_add_ps(v, rhs.v);
	return *this;
}

/**************************************************************************************************
* Subtract a quaternion and return the result
**************************************************************************************************/
inline Quaternion_16f& Quaternion_16f::operator-=(const Quaternion_16f& rhs) {
	v = _mm_sub_ps(v, rhs.v);
	return *this;
}

/**************************************************************************************************
* Multiply a quaternion by another quaternion and return the result
**************************************************************************************************/
inline Quaternion_16f& Quaternion_16f::operator*=(const Quaternion_16f& rhs) {
	float X(w*rhs.x + x*rhs.w + z*rhs.y - y*rhs.z);
	float Y(w*rhs.y + y*rhs.w + x*rhs.z - z*rhs.x);
	float Z(w*rhs.z + z*rhs.w + y*rhs.x - x*rhs.y);
	w = w*rhs.w - x*rhs.x - y*rhs.y - z*rhs.z;
	x = X;
	y = Y;
	z = Z;
	return *this;
}

/**************************************************************************************************
* Multiply a quaternion by a scalar value
**************************************************************************************************/
inline Quaternion_16f& Quaternion_16f::operator*=(const __m128& rhs) {
	v = _mm_mul_ps(v, rhs);
	return *this;
}

/**************************************************************************************************
* Multiply the quaternion by a scalar
**************************************************************************************************/
inline Quaternion_16f& Quaternion_16f::operator*=(float rhs) {
	v = _mm_mul_ps(v, _mm_load1_ps(&rhs));
	return *this;
}

/**************************************************************************************************
* Divide the quaternion by a scalar
**************************************************************************************************/
inline Quaternion_16f& Quaternion_16f::operator/=(const __m128& rhs) {
	v = _mm_div_ps(v, rhs);
	return *this;
}

/**************************************************************************************************
* Divide the quaternion by a scalar
**************************************************************************************************/
inline Quaternion_16f& Quaternion_16f::operator/=(float rhs) {
	v = _mm_div_ps(v, _mm_load1_ps(&rhs));
	return *this;
}

/**************************************************************************************************
* Return an unmodified copy of the quaternion
**************************************************************************************************/
inline Quaternion_16f Quaternion_16f::operator+(void) const {
	return *this;
}

/**************************************************************************************************
* Return a negated copy of the quaternion
**************************************************************************************************/
inline Quaternion_16f Quaternion_16f::operator-(void) const {
	return Quaternion_16f(_mm_mul_ps(v, _mm_set1_ps(-1.0f)));
}

/**************************************************************************************************
* Return the sum of two quaternions
**************************************************************************************************/
inline Quaternion_16f Quaternion_16f::operator+(const Quaternion_16f& rhs) const {
	return Quaternion_16f(_mm_add_ps(v, rhs.v));
}

/**************************************************************************************************
* Return the difference between two quaternions
**************************************************************************************************/
inline Quaternion_16f Quaternion_16f::operator-(const Quaternion_16f& rhs) const {
	return Quaternion_16f(_mm_sub_ps(v, rhs.v));
}

/**************************************************************************************************
* Return the product of two quaternions
**************************************************************************************************/
inline Quaternion_16f Quaternion_16f::operator*(const Quaternion_16f& rhs) const {
	return Quaternion_16f(	w*rhs.x + x*rhs.w + z*rhs.y - y*rhs.z,
							w*rhs.y + y*rhs.z + x*rhs.z - z*rhs.x,
							w*rhs.z + z*rhs.w + y*rhs.x - x*rhs.y,
							w*rhs.w - x*rhs.x - y*rhs.y - z*rhs.z);
}

/**************************************************************************************************
* Return a quaternion multiplied by a scalar
**************************************************************************************************/
inline Quaternion_16f Quaternion_16f::operator*(const __m128& rhs) const {
	return Quaternion_16f(_mm_mul_ps(v, rhs));
}

/**************************************************************************************************
* Multiply the quaternion by a vector (primarily used in determining axes)
**************************************************************************************************/
inline Vector3_16f Quaternion_16f::operator*(const Vector3_16f& rhs) const {
	Vector3_16f qVec(x, y, z);
	Vector3_16f uv, uuv;
	Vec3Cross_16f(&uv, &qVec, &rhs);
	Vec3Cross_16f(&uuv, &qVec, &uv);
	Vec3Scale_16f(&uv, &uv, 2.0f * w);
	Vec3Scale_16f(&uuv, &uuv, 2.0f);
	Vec3Add_16f(&qVec, &rhs, &uv);
	Vec3Add_16f(&qVec, &qVec, &uuv);
	return qVec;
}

/**************************************************************************************************
* Return a quaternion multiplied by a scalar
**************************************************************************************************/
inline Quaternion_16f Quaternion_16f::operator*(float rhs) const {
	return Quaternion_16f(_mm_mul_ps(v, _mm_load1_ps(&rhs)));
}

/**************************************************************************************************
* Return a quaternion divided by a scalar
**************************************************************************************************/
inline Quaternion_16f Quaternion_16f::operator/(const __m128& rhs) const {
	return Quaternion_16f(_mm_div_ps(v, rhs));
}

/**************************************************************************************************
* Return a quaternion divided by a scalar
**************************************************************************************************/
inline Quaternion_16f Quaternion_16f::operator/(float rhs) const {
	return Quaternion_16f(_mm_div_ps(v, _mm_load1_ps(&rhs)));
}

/**************************************************************************************************
* Multiply a quaternion by a scalar and return the result
**************************************************************************************************/
inline Quaternion_16f operator*(const __m128& lhs, const Quaternion_16f& rhs) {
	return Quaternion_16f(_mm_mul_ps(lhs, rhs.v));
}

/**************************************************************************************************
* Multiply a quaternion by a scalar and return the result
**************************************************************************************************/
inline Quaternion_16f operator*(float lhs, const Quaternion_16f& rhs) {
	return Quaternion_16f(_mm_mul_ps(_mm_load1_ps(&lhs), rhs.v));
}

/**************************************************************************************************
* Equivalence
**************************************************************************************************/
inline bool Quaternion_16f::operator==(const Quaternion_16f& rhs) const {
	return _mm_movemask_ps(_mm_cmpeq_ps(v, rhs.v)) == 0x0F;
}

/**************************************************************************************************
* Non-equivalence
**************************************************************************************************/
inline bool Quaternion_16f::operator!=(const Quaternion_16f& rhs) const {
	return _mm_movemask_ps(_mm_cmpeq_ps(v, rhs.v)) != 0x0F;
}

/**************************************************************************************************
* Sum of two quaternions
**************************************************************************************************/
inline Quaternion_16f* QuaternionAdd_16f(Quaternion_16f* pOut, const Quaternion_16f* pQ1, 
	const Quaternion_16f* pQ2) {
	pOut->v = _mm_add_ps(pQ1->v, pQ2->v);
	return pOut;
}

/**************************************************************************************************
* Difference between two quaternions
**************************************************************************************************/
inline Quaternion_16f* QuaternionSub_16f(Quaternion_16f* pOut, const Quaternion_16f* pQ1, 
	const Quaternion_16f* pQ2) {
	pOut->v = _mm_sub_ps(pQ1->v, pQ2->v);
	return pOut;
}

/**************************************************************************************************
* Scale a quaternion
**************************************************************************************************/
inline Quaternion_16f* QuaternionScale_16f(Quaternion_16f* pOut, const Quaternion_16f* pQ, 
	const __m128& s) {
	pOut->v = _mm_mul_ps(pQ->v, s);
	return pOut;
}

/**************************************************************************************************
* Scale a quaternion
**************************************************************************************************/
inline Quaternion_16f* QuaternionScale_16f(Quaternion_16f* pOut, const Quaternion_16f* pQ, float s) {
	pOut->v = _mm_mul_ps(pQ->v, _mm_load1_ps(&s));
	return pOut;
}

/**************************************************************************************************
* Length of quaternion
**************************************************************************************************/
inline float QuaternionLength_16f(const Quaternion_16f* pQ) {
	return sqrtf(pQ->x*pQ->x + pQ->y*pQ->y + pQ->z*pQ->z + pQ->w*pQ->w);
}

/**************************************************************************************************
* Squared length, or "norm"
**************************************************************************************************/
inline float QuaternionLengthSqr_16f(const Quaternion_16f* pQ) {
	return pQ->x*pQ->x + pQ->y*pQ->y + pQ->z*pQ->z + pQ->w*pQ->w;
}

/**************************************************************************************************
* Dot product
**************************************************************************************************/
inline float QuaternionDot_16f(const Quaternion_16f* pQ1, const Quaternion_16f* pQ2) {
	return pQ1->x*pQ2->x + pQ1->y*pQ2->y + pQ1->z*pQ2->z + pQ1->w*pQ2->w;
}

/**************************************************************************************************
* Set the quaternion as identity
**************************************************************************************************/
inline Quaternion_16f* QuaternionIdentity_16f(Quaternion_16f* pOut) {
	pOut->x = 0.0f;
	pOut->y = 0.0f;
	pOut->z = 0.0f;
	pOut->w = 1.0f;
	return pOut;
}

/**************************************************************************************************
* Verify if the quaternion is an identity quaternion
**************************************************************************************************/
inline bool QuaternionIsIdentity_16f(const Quaternion_16f* pQ) {
	return pQ->x == 0.0f && pQ->y == 0.0f && pQ->z == 0.0f && pQ->w == 1.0f;
}

/**************************************************************************************************
* Conjugate (-x, -y, -z, -w)
**************************************************************************************************/
inline Quaternion_16f* QuaternionConjugate_16f(Quaternion_16f* pOut, const Quaternion_16f* pQ) {
	pOut->x = -pQ->x;
	pOut->y = -pQ->y;
	pOut->z = -pQ->z;
	pOut->w = -pQ->w;
	return pOut;
}